package nautilus.tank.coord;

/**
	All matrix whom maniplating here should be (16)4x4 for matrix and (4)4x1 for vector
	http://www.opengl-tutorial.org/beginners-tutorials/tutorial-3-matrices/#The_Model_matrix
*/
public class Matrix3D{
	public static final float OnDegree = (float)(Math.PI/180);
	
	private static float[] buffer = new float[16];
	private static float[] buffer1 = new float[16];
	private static float[] buffer2 = new float[16];
	private static float[] buffer3 = new float[16];
	
	/**
		@param a Must be a square matrix
		@param level level of the A matrix
	*/
	public static float det(float[] A, int level){
		float d = 0;
		int s = -1;
		
		if (level == 1) return A[0];
		
		if( level == 2 ){
			d = (A[0] *  A[3]) - (A[1] *  A[2]);
			return d;
		}
		
		if( level == 3 ){
			d = (A[0] *  A[4] * A[8]) + (A[1] *  A[5] * A[6]) + (A[2] *  A[3] * A[7]) - 
				(A[2] *  A[4] * A[6]) - (A[0] *  A[5] * A[7]) - (A[1] *  A[3] * A[8]);
			return d;
		}
		
		float[] sub = new float[(level-1) * (level-1)];
		for(int i=0; i<level; i++){
			subMatrix(sub, A, level, i/level, i%level);
			d += s * A[i] * det(sub, level - 1);
			s = -s;
		}
		
		return d;
	}
	
	/**
		This method return a sub matrix when we dimiss row r and column c from the matrix
		A.
		
		@param result output result
		@param A a square matrix
		@param level level of A
		@param r row index dimissed
		@param c column index dimissed
	*/
	public static void subMatrix(float[] result, float[] A, int level, int r, int c){
		int k = 0;
		int len = level * level;
		for(int i=0; i<len; i++){
			if( (i/level) != r && (i%level) != c ){
				result[k] = A[i];
				k++;
			}
		}
	}
	
	public static void T(float[] a, float[] rm){		
		for(int i=0; i<4; i++)
			for(int j=0; j<4; j++)
				rm[j*4+i] = a[i*4+j];
	}
	
	public static void multiplyMM(float[] rm, float a[], float b[]){
		// v' = m x v
		float s;
		for(int i=0; i<4; i++){
			for(int j=0; j<4; j++){
				s = 0;
				for(int k=0; k<4; k++)
					s += a[i*4+k] * b[k*4+j];
					
				rm[i*4+j] = s;
			}
		}
	}
	
	
	/**
	 * rm = a x b x c
	 */
	public static void multiplyMMM(float[] rm, float a[], float b[], float[] c){
		// v' = m x v
		float[] tmp = new float[16];
		float s;
		for(int i=0; i<4; i++){
			for(int j=0; j<4; j++){
				s = 0;
				for(int k=0; k<4; k++)
					s += a[i*4+k] * b[k*4+j];
					
				tmp[i*4+j] = s;
			}
		}
		
		for(int i=0; i<4; i++){
			for(int j=0; j<4; j++){
				s = 0;
				for(int k=0; k<4; k++)
					s += tmp[i*4+k] * c[k*4+j];
					
				rm[i*4+j] = s;
			}
		}
	}
	
	public static void multiplyMV(float[] rm, float m[], float v[]){
		// v' = m x v
		float s;
		for(int i=0; i<4; i++){
			s = 0;
			for(int k=0; k<4; k++)
				s += m[i*4+k] * v[k];
			rm[i] = s;
		}
	}
	
	public static void affineTransformAA(float[] m, float[] p, float[] v){
		// v = m x p
		float s;
		for(int i=0; i<4; i++){
			s = 0;
			for(int j=0; j<4; j++)
				s += m[i*4+j]*p[j];
			v[i] = s;
		}
	}
	
	public static int ptransform(float[] Mvp, float[] m, float[] Mcam, float[] p, float[] v){
		// p' = Mvp x m x Mcam x p
		int i, j;
		float s;		
		float[] tmpM1 = new float[16];
		float[] tmpM2 = new float[16];
		
		for(i=0; i<4; i++){
			for(j=0; j<4; j++){
				s = 0;
				for(int k=0; k<4; k++)
					s += Mvp[i*4+k] * m[k*4+j];
					
				tmpM1[i*4+j] = s;
			}
		}
		
		for(i=0; i<4; i++){
			for(j=0; j<4; j++){
				s = 0;
				for(int k=0; k<4; k++)
					s += tmpM1[i*4+k] * Mcam[k*4+j];
					
				tmpM2[i*4+j] = s;
			}
		}
		
		for(i=0; i<4; i++){
			s = 0;
			for(j=0; j<4; j++)
				s += tmpM2[i*4+j]*p[j];
			v[i] = s;
		}
		
		return 0;
	}
	
	//affine transformations
	
	public static int translateM(float[] m, float tx, float ty, float tz){
		m[0] = 1;
		m[1] = 0;
		m[2] = 0;
		m[3] = 0;
		
		m[4] = 0;
		m[5] = 1;
		m[6] = 0;
		m[7] = 0;
		
		m[8] = 0;
		m[9] = 0;
		m[10] = 1;
		m[11] = 0;
		
		m[12] = tx;
		m[13] = ty;
		m[14] = tz;
		m[15] = 1;
		return 0;
	}
	
	/**
		@param theta rotate angle in radian
	*/
	public static void rotateX(float theta, float[] m){
		float cosx = (float)Math.cos(theta);
		float sinx = (float)Math.sin(theta);
		
		m[0] = 1;
		m[1] = 0;
		m[2] = 0;
		m[3] = 0;
		
		m[4] = 0;
		m[5] = cosx;
		m[6] = sinx;
		m[7] = 0;
		
		m[8] = 0;
		m[9] = -sinx;
		m[10] = cosx;
		m[11] = 0;
		
		m[12] = 0;
		m[13] = 0;
		m[14] = 0;
		m[15] = 1;
	}
	
	/**
		@param theta rotate angle in radian
	*/
	public static void rotateY(float theta, float[] m){
		float cosx = (float)Math.cos(theta);
		float sinx = (float)Math.sin(theta);

		m[0] = cosx;
		m[1] = 0;
		m[2] = -sinx;
		m[3] = 0;
		
		m[4] = 0;
		m[5] = 1;
		m[6] = 0;
		m[7] = 0;
		
		m[8] = sinx;
		m[9] = 0;
		m[10] = cosx;
		m[11] = 0;
		
		m[12] = 0;
		m[13] = 0;
		m[14] = 0;
		m[15] = 1;
	}
	
	/**
		@param theta rotate angle in radian
	*/
	public static void rotateZ(float theta, float[] m){
		float cosx = (float)Math.cos(theta);
		float sinx = (float)Math.sin(theta);
		
		m[0] = cosx;
		m[1] = sinx;
		m[2] = 0;
		m[3] = 0;
		
		m[4] = -sinx;
		m[5] = cosx;
		m[6] = 0;
		m[7] = 0;
		
		m[8] = 0;
		m[9] = 0;
		m[10] = 1;
		m[11] = 0;
		
		m[12] = 0;
		m[13] = 0;
		m[14] = 0;
		m[15] = 1;
	}
	
	/**
	
	
	*/
	public static void rotateByLineM(float[] rm, float theta, float ax, float ay, float az,
														float bx, float by, float bz){
		float[] T = new float[16];
		
		/** 1. Translate the line segment to the origin */
		Matrix3D.translateM(T, -ax, -ay, -az);
		
		/** 2. Rotate about X axis  */
	}
	
	/**
	 * 
	 * @param theta rotate angle in radian
	*/
	public static void rotateM(float[] v, float[] p, float theta, float x, float y, float z){
		
		/** Translate p to the origin T */
		buffer[0] = 1;
		buffer[1] = 0;
		buffer[2] = 0;
		buffer[3] = 0;
		buffer[4] = 0;
		buffer[5] = 1;
		buffer[6] = 0;
		buffer[7] = 0;
		buffer[8] = 0;
		buffer[9] = 0;
		buffer[10] = 1;
		buffer[11] = 0;
		buffer[12] = p[0];
		buffer[13] = p[1];
		buffer[14] = p[2];
		buffer[15] = 1;
		
		/** Initialize rotation matrix R */
		buffer1[3] = 0;
		buffer1[7] = 0;
		buffer1[11]= 0;
		buffer1[12]= 0;
		buffer1[13]= 0;
		buffer1[14]= 0;
		buffer1[15]= 1;
	    float s = (float)Math.sin(theta);
	    float c = (float)Math.cos(theta);
	    if (1.0f == x && 0.0f == y && 0.0f == z) {
	    	buffer1[5] = c;   buffer1[10]= c;
	    	buffer1[6] = s;   buffer1[9] = -s;
	        buffer1[1] = 0;   buffer1[2] = 0;
	        buffer1[4] = 0;   buffer1[8] = 0;
	        buffer1[0] = 1;
	    } else if (0.0f == x && 1.0f == y && 0.0f == z) {
	    	buffer1[0] = c;   buffer1[10]= c;
	    	buffer1[8] = s;   buffer1[2] = -s;
	    	buffer1[1] = 0;   buffer1[4] = 0;
	    	buffer1[6] = 0;   buffer1[9] = 0;
	    	buffer1[5] = 1;
	    } else if (0.0f == x && 0.0f == y && 1.0f == z) {
	    	buffer1[0] = c;   buffer1[5] = c;
	        buffer1[1] = s;   buffer1[4] = -s;
	        buffer1[2] = 0;   buffer1[6] = 0;
	        buffer1[8] = 0;   buffer1[9] = 0;
	        buffer1[10]= 1;
	    } else {
	    	float len = (float)Math.sqrt(x*x + y*y + z*z);
	        if (1.0f != len) {
	            float recipLen = 1.0f / len;
	            x *= recipLen;
	            y *= recipLen;
	            z *= recipLen;
	        }
	        float nc = 1.0f - c;
	        float xy = x * y;
	        float yz = y * z;
	
	        float zx = z * x;
	        float xs = x * s;
	        float ys = y * s;
	        float zs = z * s;
	
	        buffer1[0] = x*x*nc +  c;
	        buffer1[4] =  xy*nc - zs;
	        buffer1[8] =  zx*nc + ys;
	        buffer1[1] =  xy*nc + zs;
	        buffer1[5] = y*y*nc +  c;
	        buffer1[9] =  yz*nc - xs;
	        buffer1[2] =  zx*nc - ys;
	        buffer1[6] =  yz*nc + xs;
	        buffer1[10] = z*z*nc +  c;
	    }
	    
	    /** inverse the translation p from origin T' */
		buffer2[0] = 1;
		buffer2[1] = 0;
		buffer2[2] = 0;
		buffer2[3] = 0;
		buffer2[4] = 0;
		buffer2[5] = 1;
		buffer2[6] = 0;
		buffer2[7] = 0;
		buffer2[8] = 0;
		buffer2[9] = 0;
		buffer2[10] = 1;
		buffer2[11] = 0;
		buffer2[12] = -p[0];
		buffer2[13] = -p[1];
		buffer2[14] = -p[2];
		buffer2[15] = 1;
		
		/** M = T' . R . T */
		multiplyMMM(buffer3, buffer2, buffer1, buffer);
		float[] vt = {v[0], v[1], v[2]};
		multiplyMV(v, buffer3, vt);
	}
	
	/**
	 * 
	 * @param theta rotate angle in radian
	*/
	public static void rotateM(float[] v, float px, float py, float pz, float theta, float x, float y, float z){
		
		/** Translate p to the origin T */
		buffer[0] = 1;
		buffer[1] = 0;
		buffer[2] = 0;
		buffer[3] = 0;
		buffer[4] = 0;
		buffer[5] = 1;
		buffer[6] = 0;
		buffer[7] = 0;
		buffer[8] = 0;
		buffer[9] = 0;
		buffer[10] = 1;
		buffer[11] = 0;
		buffer[12] = px;
		buffer[13] = py;
		buffer[14] = pz;
		buffer[15] = 1;
		
		/** Initialize rotation matrix R */
		buffer1[3] = 0;
		buffer1[7] = 0;
		buffer1[11]= 0;
		buffer1[12]= 0;
		buffer1[13]= 0;
		buffer1[14]= 0;
		buffer1[15]= 1;
	    float s = (float)Math.sin(theta);
	    float c = (float)Math.cos(theta);
	    if (1.0f == x && 0.0f == y && 0.0f == z) {
	    	buffer1[5] = c;   buffer1[10]= c;
	    	buffer1[6] = s;   buffer1[9] = -s;
	        buffer1[1] = 0;   buffer1[2] = 0;
	        buffer1[4] = 0;   buffer1[8] = 0;
	        buffer1[0] = 1;
	    } else if (0.0f == x && 1.0f == y && 0.0f == z) {
	    	buffer1[0] = c;   buffer1[10]= c;
	    	buffer1[8] = s;   buffer1[2] = -s;
	    	buffer1[1] = 0;   buffer1[4] = 0;
	    	buffer1[6] = 0;   buffer1[9] = 0;
	    	buffer1[5] = 1;
	    } else if (0.0f == x && 0.0f == y && 1.0f == z) {
	    	buffer1[0] = c;   buffer1[5] = c;
	        buffer1[1] = s;   buffer1[4] = -s;
	        buffer1[2] = 0;   buffer1[6] = 0;
	        buffer1[8] = 0;   buffer1[9] = 0;
	        buffer1[10]= 1;
	    } else {
	    	float len = (float)Math.sqrt(x*x + y*y + z*z);
	        if (1.0f != len) {
	            float recipLen = 1.0f / len;
	            x *= recipLen;
	            y *= recipLen;
	            z *= recipLen;
	        }
	        float nc = 1.0f - c;
	        float xy = x * y;
	        float yz = y * z;
	
	        float zx = z * x;
	        float xs = x * s;
	        float ys = y * s;
	        float zs = z * s;
	
	        buffer1[0] = x*x*nc +  c;
	        buffer1[4] =  xy*nc - zs;
	        buffer1[8] =  zx*nc + ys;
	        buffer1[1] =  xy*nc + zs;
	        buffer1[5] = y*y*nc +  c;
	        buffer1[9] =  yz*nc - xs;
	        buffer1[2] =  zx*nc - ys;
	        buffer1[6] =  yz*nc + xs;
	        buffer1[10] = z*z*nc +  c;
	    }
	    
	    /** inverse the translation p from origin T' */
		buffer2[0] = 1;
		buffer2[1] = 0;
		buffer2[2] = 0;
		buffer2[3] = 0;
		buffer2[4] = 0;
		buffer2[5] = 1;
		buffer2[6] = 0;
		buffer2[7] = 0;
		buffer2[8] = 0;
		buffer2[9] = 0;
		buffer2[10] = 1;
		buffer2[11] = 0;
		buffer2[12] = -px;
		buffer2[13] = -py;
		buffer2[14] = -py;
		buffer2[15] = 1;
		
		/** M = T' . R . T */
		multiplyMMM(buffer3, buffer2, buffer1, buffer);
		float[] vt = {v[0], v[1], v[2]};
		multiplyMV(v, buffer3, vt);
	}
	
	/**
		@param theta rotate angle in radian
	*/
	public static void rotateM(float[] rm, float theta, float x, float y, float z){
		rm[3] = 0;
        rm[7] = 0;
        rm[11]= 0;
        rm[12]= 0;
        rm[13]= 0;
        rm[14]= 0;
        rm[15]= 1;
        float s = (float)Math.sin(theta);
        float c = (float)Math.cos(theta);
        if (1.0f == x && 0.0f == y && 0.0f == z) {
            rm[5] = c;   rm[10]= c;
            rm[6] = s;   rm[9] = -s;
            rm[1] = 0;   rm[2] = 0;
            rm[4] = 0;   rm[8] = 0;
            rm[0] = 1;
        } else if (0.0f == x && 1.0f == y && 0.0f == z) {
            rm[0] = c;   rm[10]= c;
            rm[8] = s;   rm[2] = -s;
            rm[1] = 0;   rm[4] = 0;
            rm[6] = 0;   rm[9] = 0;
            rm[5] = 1;
        } else if (0.0f == x && 0.0f == y && 1.0f == z) {
            rm[0] = c;   rm[5] = c;
            rm[1] = s;   rm[4] = -s;
            rm[2] = 0;   rm[6] = 0;
            rm[8] = 0;   rm[9] = 0;
            rm[10]= 1;
        } else {
        	float len = (float)Math.sqrt(x*x + y*y + z*z);
            if (1.0f != len) {
                float recipLen = 1.0f / len;
                x *= recipLen;
                y *= recipLen;
                z *= recipLen;
            }
            float nc = 1.0f - c;
            float xy = x * y;
            float yz = y * z;

            float zx = z * x;
            float xs = x * s;
            float ys = y * s;
            float zs = z * s;

            rm[0] = x*x*nc +  c;
            rm[4] =  xy*nc - zs;
            rm[8] =  zx*nc + ys;
            rm[1] =  xy*nc + zs;
            rm[5] = y*y*nc +  c;
            rm[9] =  yz*nc - xs;
            rm[2] =  zx*nc - ys;
            rm[6] =  yz*nc + xs;
            rm[10] = z*z*nc +  c;
        }
	}
	
	public static void scale(float sx, float sy, float sz, float[] m){
		m[0] = sx;
		m[1] = 0;
		m[2] = 0;
		m[3] = 1;
		
		m[4] = 0;
		m[5] = sy;
		m[6] = 0;
		m[7] = 0;
		
		m[8] = 0;
		m[9] = 0;
		m[10] = sz;
		m[11] = 0;
		
		m[12] = 0;
		m[13] = 0;
		m[14] = 0;
		m[15] = 1;
	}
	
	public static void shear(float xy, float yx, float xz, float zx, float yz, float zy, float[] m){
		m[0] = 1;
		m[1] = yx;
		m[2] = zx;
		m[3] = 1;
		
		m[4] = xy;
		m[5] = 1;
		m[6] = zy;
		m[7] = 0;
		
		m[8] = xz;
		m[9] = yz;
		m[10] = 1;
		m[11] = 0;
		
		m[12] = 0;
		m[13] = 0;
		m[14] = 0;
		m[15] = 1;
	}
	
	public static void reflectX(float[] m){
		m[0] = 1;
		m[1] = 0;
		m[2] = 0;
		m[3] = 0;
		
		m[4] = 0;
		m[5] = -1;
		m[6] = 0;
		m[7] = 0;
		
		m[8] = 0;
		m[9] = 0;
		m[10] = -1;
		m[11] = 0;
		
		m[12] = 0;
		m[13] = 0;
		m[14] = 0;
		m[15] = 1;
	}
	
	public static void reflectY(float[] m)	{
		m[0] = -1;
		m[1] = 0;
		m[2] = 0;
		m[3] = 0;
		
		m[4] = 0;
		m[5] = 1;
		m[6] = 0;
		m[7] = 0;
		
		m[8] = 0;
		m[9] = 0;
		m[10] = -1;
		m[11] = 0;
		
		m[12] = 0;
		m[13] = 0;
		m[14] = 0;
		m[15] = 1;
	}
	
	public static void setIdentity(float[] m)	{
		m[0] = 1;
		m[1] = 0;
		m[2] = 0;
		m[3] = 0;
		
		m[4] = 0;
		m[5] = 1;
		m[6] = 0;
		m[7] = 0;
		
		m[8] = 0;
		m[9] = 0;
		m[10] = 1;
		m[11] = 0;
		
		m[12] = 0;
		m[13] = 0;
		m[14] = 0;
		m[15] = 1;
	}
	
	public static void reflectZ(float[] m)	{
		m[0] = -1;
		m[1] = 0;
		m[2] = 0;
		m[3] = 0;
		
		m[4] = 0;
		m[5] = -1;
		m[6] = 0;
		m[7] = 0;
		
		m[8] = 0;
		m[9] = 0;
		m[10] = 1;
		m[11] = 0;
		
		m[12] = 0;
		m[13] = 0;
		m[14] = 0;
		m[15] = 1;
	}
	
	static void cross(float[] r, float[] v1, float[] v2) {
	    r[0] = v1[1]*v2[2] - v1[2]*v2[1];
	    r[1] = v1[2]*v2[0] - v1[0]*v2[2];
	    r[2] = v1[0]*v2[1] - v1[1]*v2[0];
	}
	
	/**
		@see http://3dgep.com/?p=1700
		@see http://www.songho.ca/opengl/gl_transform.html
		@see http://www.songho.ca/opengl/gl_lookattoaxes.html
	*/
	public static void lookAtM(float[] rm, float ex, float ey, float ez,
			float targetX, float targetY, float targetZ, float upX, float upY, float upZ){
		
		//get the forward vector
		float fx = targetX - ex;
		float fy = targetY - ey;
		float fz = targetY - ez;
		//Normalize f (forward vector)
		float rlf = (float)(1.0 / Math.sqrt(fx*fx + fy*fy + fz*fz));
		fx *= rlf;
		fy *= rlf;
		fz *= rlf;
		
		// compute s = f x up (x means "cross product")
		float sx = fy * upZ - fz * upY;
		float sy = fz * upX - fx * upZ;
		float sz = fx * upY - fy * upX;
		// and normalize s
		rlf = (float)(1.0 / Math.sqrt(sx*sx + sy*sy + sz*sz));
		sx *= rlf;
		sy *= rlf;
		sz *= rlf;
		
		// compute u = s x f
		float ux = sy * fz - sz * fy;
        float uy = sz * fx - sx * fz;
        float uz = sx * fy - sy * fx;
        // and normalize s
     	rlf = (float)(1.0 / Math.sqrt(ux*ux + uy*uy + uz*uz));
     	ux *= rlf;
     	uy *= rlf;
     	uz *= rlf;
		
        rm[0] = sx;
        rm[1] = ux;
        rm[2] = fx;
        rm[3] = ex;
        rm[4] = sy;
        rm[5] = uy;
        rm[6] = fy;
        rm[7] = ey;
        rm[8] = sz;
        rm[9] = uz;
        rm[10] = fz;
        rm[11] = ez;
        rm[12] = 0.0f;
        rm[13] = 0.0f;
        rm[14] = 0.0f;
        rm[15] = 1.0f;
	}
	
	/**
	 *  If an object at P1 is facing to P2, then the lookat vector is P2 - P1.
		First, Forward axis vector is simply calculated by normalizing the lookat vector
		Second, left axis vector is computed by a cross product of a specified up direction 
		vector and forward axis. This up direction vector is given to determine the roll angle of 
		the object. And, it is not necessary perpendicular to the forward axis. If we don't consider 
		roll rotation of the object, then we can use (0, 1, 0) instead. It means the object is 
		always stood straight up.

		The actual up axis vector, which is orthogonal to both forward and left axis, is computed 
		by another cross product of forward and left. Both left and up axis should be normalized 
		after cross product in order to have a unit length. 
		
		compute transform axis from object position, target and up direction
		@see http://www.songho.ca/opengl/gl_lookattoaxes.html
		@param pos this is the position where we stand, it can be understood as 
			camera position or eyes position
		@param target 
	*/
	void lookAtToAxes(float[] pos, float[] target, float[] upDir, float[] left, 
			float[] up, float[] forward) {
		double inverLen;
		// compute the forward vector
		forward[0] = target[0] - pos[0];
		forward[1] = target[1] - pos[1];
		forward[2] = target[2] - pos[2];
		inverLen = 1.0/Math.sqrt(forward[0]*forward[0] + forward[1]*forward[1] + forward[2]*forward[2]);
		//forward.normalize();
		forward[0] *= inverLen;
		forward[1] *= inverLen;
		forward[2] *= inverLen;
	
		// compute the left vector by cross product and normalize
		//left = upDir.cross(forward);
		cross(left, upDir, forward);
		//left.normalize();
		inverLen = 1.0/Math.sqrt(left[0]*left[0] + left[1]*left[1] + left[2]*left[2]);
		left[0] *= inverLen;
		left[1] *= inverLen;
		left[2] *= inverLen;
		
		// compute the orthonormal up vector
		//up = forward.cross(left);
		cross(up, forward, left);
		//up.normalize();
		inverLen = 1.0/Math.sqrt(up[0]*up[0] + up[1]*up[1] + up[2]*up[2]);
		up[0] *= inverLen;
		up[1] *= inverLen;
		up[2] *= inverLen;
	}
	
}